// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_

#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <svo/global.h>

namespace vk {
	class AbstractCamera;
}

namespace svo {

	class Feature;

	/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
	class SparseImgAlign : public vk::NLLSSolver<6, SE3>
	{
		//4*4 patch is used in sparse imige alignment
		static const int patch_halfsize_ = 2;
		static const int patch_size_ = 2 * patch_halfsize_;
		static const int patch_area_ = patch_size_*patch_size_;
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

			cv::Mat resimg_;

		SparseImgAlign(
			int n_levels,
			int min_level,
			int n_iter,
			Method method,
			bool display,
			bool verbose);

		size_t run(
			FramePtr ref_frame,
			FramePtr cur_frame);

		/// Return fisher information matrix, i.e. the Hessian of the log-likelihood
		/// at the converged state.
		Matrix<double, 6, 6> getFisherInformation();

	protected:
		FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
		FramePtr cur_frame_;            //!< only the image is known!
		int level_;                     //!< current pyramid level on which the optimization runs.
		bool display_;                  //!< display residual image.
		int max_level_;                 //!< coarsest pyramid level for the alignment.
		int min_level_;                 //!< finest pyramid level for the alignment.

		// cache:
		Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
		bool have_ref_patch_cache_;
		cv::Mat ref_patch_cache_;
		std::vector<bool> visible_fts_;

		void precomputeReferencePatches();
		virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false);
		virtual int solve();
		virtual void update(const ModelType& old_model, ModelType& new_model);
		virtual void startIteration();
		virtual void finishIteration();
	};

} // namespace svo

#endif // SVO_SPARSE_IMG_ALIGN_H_
